#include <Arduino.h>
#include "wiring_analog.h"
#include "analog.h"

#if !defined(ADC_RESOLUTION_16B)
#define MAX_ADC_RESOLUTION 12
#else
#define MAX_ADC_RESOLUTION 16
#endif

#define MAX_PWM_RESOLUTION 16

static int _writeResolution = PWM_RESOLUTION;
static int _internalWriteResolution = PWM_RESOLUTION;

static int _readResolution = ADC_RESOLUTION;
static int _internalReadResolution = 12;

static uint32_t _writeFreq = PWM_FREQUENCY;

static inline uint32_t mapResolution(uint32_t value, uint32_t from, uint32_t to)
{
    if (from != to)
    {
        if (from > to)
        {
            value = (value < (uint32_t)(1 << (from - to))) ? 0 : ((value + 1) >> (from - to)) - 1;
        }
        else
        {
            if (value != 0)
            {
                value = ((value - 1) << (to - from)) - 1;
            }
        }
    }
    return value;
}

void analogReference(eAnalogReference ulMode)
{
    UNUSED(ulMode);
}

void analogWrite(uint32_t ulPin, uint32_t ulValue)
{
    PinName p = digitalPinToPinName(ulPin);
    if (p != NC)
    {
        if (pin_in_pinmap(p, PinMap_PWM))
        {
            ulValue = mapResolution(ulValue, _writeResolution, _internalWriteResolution);
            pwm_start(p, _writeFreq, ulValue, _internalWriteResolution);
        }
        else
        {
            pinMode(ulPin, OUTPUT);
            ulValue = mapResolution(ulValue, _writeResolution, 8);
            if (ulValue < 128)
            {
                digitalWrite(ulPin, LOW);
            }
            else
            {
                digitalWrite(ulPin, HIGH);
            }
        }
    }
}

uint32_t analogRead(uint32_t ulPin)
{
    uint32_t value = 0;
    PinName p = analogInputToPinName(ulPin);
    if (p != NC)
    {
        value = adc_read_value(p, _internalReadResolution);
        value = mapResolution(value, _internalReadResolution, _readResolution);
    }

    return value;
}

void analogReadResolution(int res)
{
    if ((res > 0) && (res <= 32))
    {
        _readResolution = res;
        _internalReadResolution = _readResolution;
        if (_readResolution > MAX_ADC_RESOLUTION)
        {
            _internalReadResolution = MAX_ADC_RESOLUTION;
        }
        else
        {
            _internalReadResolution = 12;
        }
    }
}

void analogWriteResolution(int res)
{
    if ((res > 0) && (res <= 32))
    {
        _writeResolution = res;
        if (_writeResolution > MAX_ADC_RESOLUTION)
            _internalWriteResolution = MAX_ADC_RESOLUTION;
        else
            _internalWriteResolution = _writeResolution;
    }
}

void analogWriteFrequency(uint32_t freq)
{
    _writeFreq = freq;
}

void analogOutputInit(void)
{
}
